
import numpy
import requests
from typing import Optional, Dict, Any
from threading import Lock
import time
import cv2
from t.th import Th, Fsm
from drvs.pca9685 import PCA9685
from drvs.mpu6050 import MPU6050

def safe_request(url: str,
                 method: str = 'GET',
                 timeout: int = 10,
                 retries: int = 3,
                 headers: Optional[Dict[str, str]] = None,
                 **kwargs) -> Optional[requests.Response]:
    """
    带错误处理的 requests 访问封装
    自动重试、超时保护、异常捕获
    """
    if headers is None:
        headers = {
            'User-Agent': 'Mozilla/5.0 (compatible; TraeAI-Bot/1.0)'
        }

    for attempt in range(1, retries + 1):
        try:
            resp = requests.request(method, url,
                                    headers=headers,
                                    timeout=timeout,
                                    **kwargs)
            resp.raise_for_status()          # 抛出 4xx/5xx 异常
            return resp
        except requests.exceptions.HTTPError as e:
            print(f"[HTTP 错误] {url} -> {e} (第 {attempt} 次)")
        except requests.exceptions.Timeout:
            print(f"[超时] {url} 第 {attempt} 次")
        except requests.exceptions.ConnectionError:
            print(f"[连接失败] {url} 第 {attempt} 次")
        except requests.exceptions.RequestException as e:
            print(f"[请求异常] {url} -> {e} 第 {attempt} 次")
        except Exception as e:
            print(f"[未知异常] {url} -> {e} 第 {attempt} 次")

        if attempt < retries:
            time.sleep(0.5 * attempt)        # 简单退避
    return None


class CamTh(Th):
    '''相机线程'''
    def _wait(self, fsm: Fsm):
        fsm.waitForChange()
    def _start(self, fsm: Fsm):
        for _ in range(self.pool_max):
            self.pool.append(numpy.zeros((480, 640, 3), dtype=numpy.uint8))

        self.camera = cv2.VideoCapture(self.cam_id) # 初始化相机
        if not self.camera.isOpened(): # 检查相机是否成功打开
            print(f"无法打开相机 {self.cam_id}")
            fsm.setState('Stop')
            return
        # 设置相机参数
        self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        fsm.setState('Run')
    def _run(self, fsm: Fsm):
        with self.Lock:
            success, frame = self.camera.read()
            if not success:
                print("相机读取失败")
                fsm.setState('Wait') # 跳转到Wait状态，而不是直接停止
                return
            
            # 确保frame是numpy数组格式
            if frame is not None:

                # 检查pool是否已满
                self.pool.append(frame)
                if len(self.pool) > self.pool_max:  # 执行切片
                    self.pool = self.pool[-self.pool_max:]

                # 直接使用numpy数组显示图像
                #cv2.imshow('cam', frame)
            
            # 必须调用waitKey才能显示窗口
            #if cv2.waitKey(1) & 0xFF == ord('q'):
            #    fsm.setState('Stop')

    def _join(self, fsm: Fsm):
        self.pool = []
        self.camera.release()
        #cv2.destroyAllWindows()

    def __init__(self, cam_id: int=0, pool_max: int=40) -> None:
        super().__init__(self._wait, self._start, self._run, self._join)
        self.cam_id = cam_id
        self.pool = []
        self.pool_max = pool_max
        self.Lock = Lock()
        #self.Cond = Condition(self.Lock)
    
    def get_pool(self) -> list:
        with self.Lock:
            return self.pool.copy()

class I2cTh(Th):
    '''I2C线程'''

    def _wait(self, fsm: Fsm):
        fsm.waitForChange()

    def _start(self, fsm: Fsm):
        for _ in range(self.pool_max):
            self.pool.append({
                'roll': 0.0,
                'pitch': 0.0,
                'yaw': 0.0,
            })

        self.pca = PCA9685(self.i2c_bus)
        self.pca.set_freq(50)

        self.mpu = MPU6050(self.i2c_bus)
        self.mpu.calibrate()

        fsm.setState('Run')

    def _run(self, fsm: Fsm):
        with self.Lock:
            self.mpu.reset_orientation()
            dd = self.mpu.calculate_angles()
            self.pool.append(dd)
            if len(self.pool) > self.pool_max:  # 执行切片
                self.pool = self.pool[-self.pool_max:]
            
    def _join(self, fsm: Fsm):
        self.pool = []
        del self.mpu
        del self.pca
    
    def __init__(self, i2c_bus: int=1, pool_max: int=40) -> None:
        super().__init__(self._wait, self._start, self._run, self._join)
        self.i2c_bus = i2c_bus
        self.pool = []
        self.pool_max = pool_max
        self.Lock = Lock()
    
    def set_servo(self, servo_id: int, angle: float):
        '''设置舵机角度'''
        with self.Lock:
            self.pca.set_angle(servo_id, angle)
    
    def get_pool(self) -> list:
        with self.Lock:
            return self.pool.copy()

